/**
 * @file rtk_ndt_mapper.hpp
 * @brief RTK-corrected NDT (Normal Distributions Transform) 映射器的头文件
 * 
 * 该文件定义了基于NDT算法的点云映射器，并使用RTK数据进行位姿矫正，用于构建高精度的点云地图。
 * 通过订阅激光雷达点云数据和RTK定位数据，使用NDT算法进行点云配准，并利用RTK数据进行位姿矫正，
 * 逐步构建环境的3D点云地图。
 */

#ifndef AUTO_NAVIGATION_RTK_NDT_MAPPER_HPP
#define AUTO_NAVIGATION_RTK_NDT_MAPPER_HPP

#include <ros/ros.h>
#include <ros/package.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_eigen/tf2_eigen.h>
#include <std_srvs/Trigger.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <pcl/point_types.h>
#include <pcl/registration/ndt.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>

#include <string>
#include <memory>
#include <Eigen/Dense>

namespace auto_navigation {

/**
 * @class RTKNDTMapper
 * @brief 基于RTK-corrected NDT算法的点云映射器类
 * 
 * 该类实现了基于NDT (Normal Distributions Transform) 算法的点云映射功能，并使用RTK数据进行位姿矫正。
 * 通过连续处理激光雷达点云数据和RTK定位数据，使用NDT算法进行点云配准，并利用RTK数据进行位姿矫正，
 * 逐步构建环境的3D点云地图。
 * 
 * ROS话题:
 * 订阅:
 *   - velodyne_points (sensor_msgs/PointCloud2): 激光雷达点云数据
 *   - rtk_odometry_node/odom (nav_msgs/Odometry): RTK定位数据
 * 
 * 发布:
 *   - rtk_ndt/map (sensor_msgs/PointCloud2): 构建的地图点云
 *   - rtk_ndt/current_pose (geometry_msgs/PoseStamped): 当前估计的机器人位姿
 *   - rtk_ndt/current_scan (sensor_msgs/PointCloud2): 当前处理后的激光雷达点云
 * 
 * 服务:
 *   - save_map (std_srvs/Trigger): 保存当前构建的地图到文件
 * 
 * TF转换:
 *   - map -> base_link: 发布地图到机器人基座的转换
 */
class RTKNDTMapper {
public:
    /**
     * @brief 构造函数
     * @param nh ROS节点句柄
     * @param private_nh ROS私有节点句柄
     */
    RTKNDTMapper(ros::NodeHandle& nh, ros::NodeHandle& private_nh);
    
    /**
     * @brief 析构函数
     * 在析构时自动保存地图
     */
    ~RTKNDTMapper();

private:
    // ROS相关
    ros::NodeHandle& nh_;
    ros::NodeHandle& private_nh_;
    
    // 订阅器
    ros::Subscriber points_sub_;    // 激光雷达点云订阅
    ros::Subscriber rtk_odom_sub_;  // RTK定位数据订阅
    
    // 消息过滤器（用于同步点云和RTK数据）
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> SyncPolicy;
    std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> sync_points_sub_;
    std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> sync_rtk_odom_sub_;
    std::unique_ptr<message_filters::Synchronizer<SyncPolicy>> sync_;
    
    // 发布器
    ros::Publisher map_pub_;        // 地图发布
    ros::Publisher current_pose_pub_; // 当前位姿发布
    ros::Publisher current_scan_pub_; // 当前点云发布
    
    // 服务
    ros::ServiceServer save_map_service_;  // 添加保存地图服务
    
    // TF相关
    tf2_ros::TransformBroadcaster tf_broadcaster_;
    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    // PCL NDT对象
    pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt_;
    
    // 点云和地图数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr map_cloud_;      // 全局地图点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr current_cloud_;  // 当前帧点云
    
    // 位姿相关
    Eigen::Matrix4f current_pose_;    // 当前位姿
    Eigen::Matrix4f previous_pose_;   // 上一帧位姿
    Eigen::Matrix4f rtk_pose_;        // RTK位姿
    bool is_first_frame_;            // 是否是第一帧
    bool has_rtk_data_;              // 是否有RTK数据
    
    // NDT参数
    double voxel_leaf_size_;     // 体素滤波大小
    double ndt_resolution_;      // NDT网格分辨率
    double ndt_step_size_;       // NDT优化步长
    double ndt_epsilon_;         // NDT收敛阈值
    int max_iterations_;         // 最大迭代次数
    
    // RTK参数
    double rtk_weight_;          // RTK权重（0-1之间，0表示完全使用NDT，1表示完全使用RTK）
    double rtk_max_distance_;    // RTK最大距离阈值（超过此距离则不使用RTK数据）
    
    // 坐标系参数
    std::string map_frame_id_;     // 地图坐标系
    std::string base_frame_id_;    // 机器人基座坐标系
    std::string lidar_frame_id_;   // 激光雷达坐标系
    
    // 回调函数
    void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& input_cloud);
    void rtkOdomCallback(const nav_msgs::Odometry::ConstPtr& rtk_odom);
    void syncCallback(const sensor_msgs::PointCloud2::ConstPtr& input_cloud, 
                     const nav_msgs::Odometry::ConstPtr& rtk_odom);
    bool saveMapCallback(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res);
    
    // 功能函数
    void initializeParams();
    bool alignPointCloud();
    void updateMap();
    void publishMap();
    void publishPose();
    void publishCurrentScan();
    bool getInitialGuess(Eigen::Matrix4f& initial_guess);
    void updatePoseWithRTK();
    Eigen::Matrix4f odomToEigenMatrix(const nav_msgs::Odometry::ConstPtr& odom_msg);
    
    // 辅助函数
    void downsamplePointCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& input_cloud,
                            pcl::PointCloud<pcl::PointXYZ>::Ptr& output_cloud,
                            double leaf_size);

    bool saveMap(const std::string& filename);
};

} // namespace auto_navigation

#endif // AUTO_NAVIGATION_RTK_NDT_MAPPER_HPP 